#!/usr/bin/env python3
# region # 导入包
import os
import struct
import threading
import sys
import time
import math
import re
import socket
import serial
from datetime import datetime
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QComboBox, QLineEdit, QListView, QLabel, QScrollArea
from PyQt5.QtCore import QThread, pyqtSignal, Qt, QPoint, QEvent, QObject
from PyQt5.QtGui import QStandardItemModel, QStandardItem, QMouseEvent, QPixmap, QImage
import cv2
import numpy as np
import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32
from sensor_msgs.msg import LaserScan, Imu
from geometry_msgs.msg import TransformStamped, Twist, PoseWithCovarianceStamped, Point
from tf2_ros import TransformBroadcaster
import tf_transformations
from agv_control.widget import Ui_widget_main, Ui_widget_control
from agv_control.parameter import AgvParameters, DeviceParameters
import agv_control.maps
import inspect
# endregion
# region # 全局变量
AgvState = {}
# endregion
# region # 自定义函数及类


def extract_ip():  # 获取本机IP
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    finally:
        st.close()
    return IP


def HexStrAddSpace(HexStr):  # 十六进制字符串标准化打印
    out_data = ''
    for i, s in enumerate(HexStr):
        if ((i % 2 == 0) & (i > 0)):
            out_data += ' '
            out_data += s
        else:
            out_data += s
    return out_data.upper()


def show_text(function):

    def wrapped(self, *args, **kwargs):
        if self.vars["showTextLock"]:
            self.vars["showTextLock"] = False
            result = function(self, *args, **kwargs)
            items = self.get_selected()
            l = len(items)
            l_ = self.vars["listViewModel"].rowCount() - 1
            self.vars["listViewModel"].item(0).setCheckState(
                Qt.Checked if l == l_ else Qt.Unchecked if l == 0 else Qt.PartiallyChecked)
            self.vars["lineEdit"].setText("(全选)" if l == l_ else "(无选择)" if l == 0 else ";".join(
                (item.text() for item in items)))
            self.vars["showTextLock"] = True
        else:
            result = function(self, *args, **kwargs)
        return result

    return wrapped


class QComboCheckBox(QComboBox):

    class MyListView(QListView):

        def __init__(self, parent: QWidget = None, vars=None):
            super().__init__(parent)
            self.vars = vars

        def mousePressEvent(self, event: QMouseEvent):
            self.vars["lock"] = False
            super().mousePressEvent(event)

        def mouseDoubleClickEvent(self, event: QMouseEvent):
            self.vars["lock"] = False
            super().mouseDoubleClickEvent(event)

    def __init__(self, parent: QWidget = None):
        super().__init__(parent)
        self.vars = dict()
        self.vars["lock"] = True
        self.vars["showTextLock"] = True
        # 装饰器锁，避免批量操作时重复改变lineEdit的显示
        self.vars["lineEdit"] = QLineEdit(self)
        self.vars["lineEdit"].setReadOnly(True)
        self.vars["listView"] = self.MyListView(self, self.vars)
        self.vars["listViewModel"] = QStandardItemModel(self)
        self.setModel(self.vars["listViewModel"])
        self.setView(self.vars["listView"])
        self.setLineEdit(self.vars["lineEdit"])

        self.activated.connect(self.__show_selected)

        self.add_item("(全选)")

    def count(self):
        # 返回子项数
        return super().count() - 1

    @show_text
    def add_item(self, text: "str"):
        # 根据文本添加子项
        item = QStandardItem()
        item.setText(text)
        item.setCheckable(True)
        item.setCheckState(Qt.Checked)
        self.vars["listViewModel"].appendRow(item)

    @show_text
    def add_items(self, texts: "tuple or list"):
        # 根据文本列表添加子项
        for text in texts:
            self.add_item(text)

    @show_text
    def clear_items(self):
        # 清空所有子项
        self.vars["listViewModel"].clear()
        self.add_item("(全选)")

    def find_index(self, index: "int"):
        # 根据索引查找子项
        return self.vars["listViewModel"].item(index if index < 0 else index + 1)

    def find_indexs(self, indexs: "tuple or list"):
        # 根据索引列表查找子项
        return [self.find_index(index) for index in indexs]

    def find_text(self, text: "str"):
        # 根据文本查找子项
        tempList = self.vars["listViewModel"].findItems(text)
        tempList.pop(0) if tempList and tempList[0].row() == 0 else tempList
        return tempList

    def find_texts(self, texts: "tuple or list"):
        # 根据文本列表查找子项
        return {text: self.find_text(text) for text in texts}

    def get_text(self, index: "int"):
        # 根据索引返回文本
        return self.vars["listViewModel"].item(index if index < 0 else index + 1).text()

    def get_texts(self, indexs: "tuple or list"):
        # 根据索引列表返回文本
        return [self.get_text(index) for index in indexs]

    def change_text(self, index: "int", text: "str"):
        # 根据索引改变某一子项的文本
        self.vars["listViewModel"].item(
            index if index < 0 else index + 1).setText(text)

    @show_text
    def select_index(self, index: "int", state: "bool" = True):
        # 根据索引选中子项，state=False时为取消选中
        self.vars["listViewModel"].item(
            index if index < 0 else index + 1).setCheckState(Qt.Checked if state else Qt.Unchecked)

    @show_text
    def select_indexs(self, indexs: "tuple or list", state: "bool" = True):
        # 根据索引列表选中子项，state=False时为取消选中
        for index in indexs:
            self.select_index(index, state)

    @show_text
    def select_text(self, text: "str", state: "bool" = True):
        # 根据文本选中子项，state=False时为取消选中
        for item in self.find_text(text):
            item.setCheckState(Qt.Checked if state else Qt.Unchecked)

    @show_text
    def select_texts(self, texts: "tuple or list", state: "bool" = True):
        # 根据文本列表选中子项，state=False时为取消选中
        for text in texts:
            self.select_text(text, state)

    @show_text
    def select_reverse(self):
        # 反转选择
        if self.vars["listViewModel"].item(0).checkState() == Qt.Unchecked:
            self.select_all()
        elif self.vars["listViewModel"].item(0).checkState() == Qt.Checked:
            self.select_clear()
        else:
            for row in range(1, self.vars["listViewModel"].rowCount()):
                self.__select_reverse(row)

    def __select_reverse(self, row: "int"):
        item = self.vars["listViewModel"].item(row)
        item.setCheckState(Qt.Unchecked if item.checkState()
                           == Qt.Checked else Qt.Checked)

    @show_text
    def select_all(self):
        # 全选
        for row in range(0, self.vars["listViewModel"].rowCount()):
            self.vars["listViewModel"].item(row).setCheckState(Qt.Checked)

    @show_text
    def select_clear(self):
        # 全不选
        for row in range(0, self.vars["listViewModel"].rowCount()):
            self.vars["listViewModel"].item(row).setCheckState(Qt.Unchecked)

    @show_text
    def remove_index(self, index: "int"):
        # 根据索引移除子项
        return self.vars["listViewModel"].takeRow(index if index < 0 else index + 1)

    @show_text
    def remove_indexs(self, indexs: "tuple or list"):
        # 根据索引列表移除子项
        return [self.remove_index(index) for index in sorted(indexs, reverse=True)]

    @show_text
    def remove_text(self, text: "str"):
        # 根据文本移除子项
        items = self.find_text(text)
        indexs = [item.row() for item in items]
        return [self.vars["listViewModel"].takeRow(index) for index in sorted(indexs, reverse=True)]

    @show_text
    def remove_texts(self, texts: "tuple or list"):
        # 根据文本列表移除子项
        return {text: self.remove_text(text) for text in texts}

    def get_selected(self):
        # 获取当前选择的子项
        items = list()
        for row in range(1, self.vars["listViewModel"].rowCount()):
            item = self.vars["listViewModel"].item(row)
            if item.checkState() == Qt.Checked:
                items.append(item)
        return items

    def is_all(self):
        # 判断是否是全选
        return True if self.vars["listViewModel"].item(0).checkState() == Qt.Checked else False

    def sort(self, order=Qt.AscendingOrder):
        # 排序，默认正序
        self.vars["listViewModel"].sort(0, order)

    @show_text
    def __show_selected(self, index):
        if not self.vars["lock"]:
            if index == 0:
                if self.vars["listViewModel"].item(0).checkState() == Qt.Checked:
                    self.select_clear()
                else:
                    self.select_all()
            else:
                self.__select_reverse(index)

            self.vars["lock"] = True

    def hidePopup(self):
        if self.vars["lock"]:
            super().hidePopup()


def extract_ip():  # 获取本机IP
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    finally:
        st.close()
    return IP


def get_timestamp():  # 获取当前时间戳
    current_time = datetime.now().strftime('%Y-%m-%dT%H:%M:%S+08:00')
    return current_time


def get_time():  # 获取本机时间
    current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
    return current_time


def update_headerId(data):
    data += 1
    if data > 0xFFFFFFFF:
        data = 0
    return data


def angle_rectify(angle):
    rectify = (angle + 180) % 360 - 180
    return rectify


def floatToList(float_value):  # 小数转字符列表
    # 将浮点数转换为二进制表示
    # binary = struct.pack('!f', float_value)#参数'!f'表示使用大端字节序
    binary = struct.pack('f', float_value)
    # 将二进制表示转换为十六进
    hex_array = [int(b) for b in binary]  # int整数表示  hex字符表示
    return hex_array


def listToFloat(list):  # 字符列表转小数
    return round(struct.unpack('<f', struct.pack('4B', *list))[0],
                 5)  # <表示小端字节序 f：这指定了要解析的数据类型为单精度浮点数（float）'4'：这表示接下来的格式字符将被重复四次。'B'：这是单个格式字符，表示无符号字符（unsigned char）。


def SumCheck(data, length):  # 按字节计算校验和
    checksum = 0
    for i in range(0, length):
        checksum += data[i]
        checksum &= 0xFF  # 强制截断
    return checksum


def get_exe_directory():
    # 检查是否是被打包后的环境
    if getattr(sys, 'frozen', False):
        # 如果是被打包后的环境，使用 sys.executable 获取可执行文件路径
        exe_path = sys.executable
    else:
        # 如果不是被打包后的环境，使用 __file__ 获取脚本路径
        exe_path = os.path.abspath(__file__)

    # 获取可执行文件所在的目录
    exe_directory = os.path.dirname(exe_path)
    return exe_directory


class thread_testTime(QThread):

    def __init__(self):
        super().__init__()

    def run(self):  # 该类模拟后台进程更新时间
        while True:
            time.sleep(0.01)
            print(get_time())


# endregion
#
# region # 自定义窗口


class widget_control(QWidget, Ui_widget_control.Ui_Form):  # 清洗机

    def __init__(self):
        super().__init__()
        self.setupUi(self)
# endregion
# region # 自定义类


class SerialPort(QThread):
    data_received = pyqtSignal(str, int, bytes)

    def __init__(self, port, baudrate=9600, timeout=0.1, device_type=0):
        """
        初始化串口对象

        :param port: 串口名称，例如 'COM3' 或 '/dev/ttyUSB0'
        :param baudrate: 波特率，默认 9600
        :param timeout: 读取超时时间，默认 1 秒
        """
        super().__init__()
        self.port = port
        self.baudrate = baudrate
        self.timeout = timeout
        self.serial = None
        self.identify = port
        self.device_type = device_type

    def run(self):
        self.open()
        while self.is_open():
            try:
                if self.serial.in_waiting:
                    data = self.serial.read(256)
                    if not data or data == 'exit':
                        print("exit")
                        self.close()
                        break
                    else:
                        # print(data)
                        # print(self.identify)
                        # print(self.device_type)
                        self.data_received.emit(
                            self.identify, self.device_type, data)
            except Exception as e:
                print(f"串口读取错误: {e}")
                self.close()
                break

    def open(self):
        """打开串口"""
        try:
            self.serial = serial.Serial(
                self.port, self.baudrate, timeout=self.timeout)
            # self.serial.reset_input_buffer(512)
            # import termios
            # import fcntl
            # fd = self.serial.fileno()
            # print(fd)
            # buffer_size = 4096  # 设置缓冲区大小为 4096 字节
            # fcntl.ioctl(fd, termios.TIOCSETF, termios.B4096)
            print(f"串口 {self.port} 已打开")
        except serial.SerialException as e:
            print(f"无法打开串口 {self.port}: {e}")

    def is_open(self):
        """检查串口是否已打开"""
        return self.serial and self.serial.is_open

    def close(self):
        """关闭串口"""
        if self.serial and self.serial.is_open:
            self.serial.close()
            print(f"串口 {self.port} 已关闭")
        else:
            print(f"串口 {self.port} 未打开")

    def write(self, data):
        """
        写入数据到串口

        :param data: 要写入的数据，字符串或字节
        """
        if not self.is_open():
            print("串口未打开")
            self.open()
            return

        if isinstance(data, str):
            data = data.encode('utf-8')  # 将字符串转换为字节
        try:
            self.serial.write(data)
        except serial.SerialException as e:
            print(f"写入数据时出错: {e}")
            self.open()


class Odometer(QThread):
    # region # sigal
    sig_updateAgvPos = pyqtSignal(float, float, float)
    sig_updateAgvSpeed = pyqtSignal(float, float)
    sig_updateQrcode = pyqtSignal(int, float)
    sig_updateStandly = pyqtSignal(float, float, float)
    sig_updateMotionDone = pyqtSignal()
    # endregion

    def __init__(self):
        super().__init__()
        self.control_mode = 'mannul'
        # region # 里程计相关
        self.agv_posX = AgvState['agvPosition']['x']
        self.agv_posY = AgvState['agvPosition']['y']
        self.agv_theta = AgvState['agvPosition']['theta']
        self.agv_angle = self.agv_theta * 180 / math.pi
        self.linearSpeed_F = 0
        self.angularSpeed_F = 0
        self.multiLinearSpeed = 1  # 线速度倍率(提高模拟效率)
        self.multiangularSpeed = 1  # 角速度倍率(提高模拟效率)
        self.start_time = time.time()
        # self.execution_time = 0
        self.mode_motion = 'stop'  # 'stop' 'move' 'moveToNode' 'rotate' 'rotateToNode'
        self.startNode_x = 0  # 起始点x
        self.startNode_y = 0  # 起始点y
        self.targetNode_x = 0  # 目标点x
        self.targetNode_y = 0  # 目标点y
        self.planDistance = 0  # 距离目标点距离
        self.decDistance = 0  # 提前减速距离
        self.flag_slowDown = False  # 是否开始减速
        self.flag_enSlowDown = True  # 是否需要减速
        self.creepLinearSpeed = 0.03  # 爬行线速度
        self.creepDistance = 0.03  # 爬行距离

        self.rotateTheta_T = 0  # 目标角度
        self.planTheta_last = 0  # 上次距离目标点角度
        self.decTheta = 0  # 提前减速角度
        self.creepangularSpeed = 0.03  # 爬行角速度
        self.creepTheta = 0.03  # 爬行角度
        self.flag_simulate = False  # 模拟模式
        self.flag_vel_feedback = 1  # 使用反馈速度
        # endregion
        # region # 速度曲线相关
        self.MotorDriveType = "CAN"
        self.flag_controlSpeed = 0
        self.linearSpeed_T = 0
        self.linearSpeed_C = 0
        self.linearSpeed_T_last = 0
        self.agv_max_linearSpeed = 1.2
        self.factor_sysDelay = 1  # python代码执行效率低，定时10ms循环执行只能到30-50ms，提高AGV模拟器加减速度
        self.agv_acceleration = 0.7 * self.multiLinearSpeed * self.factor_sysDelay
        self.agv_deceleration = 0.7 * self.multiLinearSpeed * self.factor_sysDelay
        self.angularSpeed_T = 0
        self.angularSpeed_C = 0
        self.angularSpeed_T_last = 0
        self.agv_max_angularSpeed = 1.2
        self.agv_RateAcceleration = 0.7 * self.multiangularSpeed * self.factor_sysDelay
        self.agv_RateDeceleration = 0.7 * self.multiangularSpeed * self.factor_sysDelay
        self.time_period = 20
        self.time_interval = self.time_period / 1000
        # endregion
        # region # IMU修正
        self.imu_angle = 0
        self.imu_offset = 0
        self.imu_theta = 0  # IMU原始角度
        self.offset_initialpose_theta = 0  # rviz初始设定角度
        # endregion
        # region # standly纠偏

        self.K_Cross = 0  # 横向误差系数
        self.K_LowVelo = 0  # 低速阻尼
        self.K_trace_angle = 0  # 轨迹纠偏系数
        self.K_cross_angle = 0  # 航向角纠偏系数
        self.path_direction = 0  # 路径方向
        self.flag_enStandly = True  # 路径方向
        self.heading_angle = 0  # 航向角
        self.flag_car_backward = False  # 是否倒车
        # endregion

    def run(self):
        while True:
            start_time = time.time()
            self.odometer()
            self.speedControl()
            end_time = time.time()
            execution_time = end_time - start_time
            # print(f"执行时间: {execution_time:.6f} 秒")
            sleep_time = self.time_interval - 0.000 - \
                execution_time  # 理论time_interval 实际测试-0.001
            if sleep_time > 0:
                time.sleep(sleep_time)

    def odometer(self):  # 里程计
        if self.angularSpeed_F != 0 or self.linearSpeed_F != 0:
            # region # 更新AGV位置
            if self.angularSpeed_F != 0 and self.flag_simulate:
                div_theta = self.angularSpeed_F * self.time_interval * self.multiangularSpeed
                # 里程计计算角度
                self.agv_theta += div_theta
                # 确保偏航角差在[-pi, pi]范围内
                self.agv_theta = (
                    self.agv_theta + math.pi) % (2 * math.pi) - math.pi
                self.agv_angle = self.agv_theta * 180 / math.pi
            if self.linearSpeed_F != 0:
                div_linearSpeed = self.linearSpeed_F * self.time_interval * self.multiLinearSpeed
                self.agv_posX += div_linearSpeed * math.cos(self.agv_theta)
                self.agv_posY += div_linearSpeed * math.sin(self.agv_theta)
            self.sig_updateAgvPos.emit(
                self.agv_posX, self.agv_posY, self.agv_theta)
            # endregion
            if self.mode_motion == 'moveToNode':
                x1 = self.agv_posX
                y1 = self.agv_posY
                x2 = self.targetNode_x
                y2 = self.targetNode_y
                x3 = self.startNode_x
                y3 = self.startNode_y
                A = (x2, y2)  # 终点
                B = (x1, y1)  # 当前
                C = (x3, y3)  # 起点

                # 横向距离
                distance_direction, distance_cross = self.projection_distance(
                    A, B, C)  # 投影距离,横向距离
                distance_cross = -distance_cross
                if abs(distance_cross) > DeviceParameters.max_distance_cross:
                    self.securityProtect(1)
                else:
                    # 横向纠偏角
                    if distance_direction < 0.05:
                        angle_cross = 0
                    else:
                        angle_cross = math.atan2(
                            self.K_Cross * distance_cross, abs(self.linearSpeed_F) + self.K_LowVelo)
                    # 航向角
                    if self.flag_car_backward:
                        self.heading_angle = self.agv_angle + 180
                    else:
                        self.heading_angle = self.agv_angle
                    self.heading_angle = angle_rectify(self.heading_angle)
                    # 航向角偏差
                    trace_angle_deviation = self.path_direction - self.heading_angle
                    trace_angle_deviation = angle_rectify(
                        trace_angle_deviation)
                    if abs(trace_angle_deviation) > DeviceParameters.max_trace_angle_deviation:
                        self.securityProtect(2)
                    else:
                        # 纠偏角速度(弧度/秒)
                        self.angularSpeed_T = self.K_trace_angle * trace_angle_deviation / \
                            180 * math.pi + self.K_cross_angle * angle_cross
                        if self.flag_enStandly == False:
                            self.angularSpeed_T = 0

                        self.sig_updateStandly.emit(
                            round(distance_cross, 3)*1000, round(trace_angle_deviation, 3), round(self.angularSpeed_T, 3))
                self.flag_controlSpeed = 1

                # 到达目标点
                if distance_direction < 0.005:
                    print(
                        f'到达目标点,本次距离{distance_direction:.3f}m,当前速度{self.linearSpeed_F:.3f}m/s')
                    if self.flag_enSlowDown:
                        self.linearSpeed_T = 0
                        self.angularSpeed_T = 0
                        self.flag_controlSpeed = 1
                    self.sig_updateMotionDone.emit()
                    self.mode_motion = 'stop'
                    self.flag_slowDown = False
                # 接近目标点
                elif distance_direction < self.decDistance and self.flag_slowDown == False and self.flag_enSlowDown:
                    print(f'开始减速{distance_direction:.3f}m')
                    if self.linearSpeed_F >= 0:
                        self.linearSpeed_T = self.creepLinearSpeed
                    else:
                        self.linearSpeed_T = -self.creepLinearSpeed

                    self.angularSpeed_T = 0
                    self.flag_controlSpeed = 1
                    self.flag_slowDown = True
            elif self.mode_motion == 'rotateToNode':
                planTheta = abs(self.calculate_theta_deviation(
                    self.agv_theta, self.rotateTheta_T))
                # 到达目标角
                if planTheta < 0.005 or planTheta - self.planTheta_last > 0.001:
                    if planTheta < 0.005:
                        print(
                            f'到达目标角度,本次偏差弧度{planTheta:.3f},当前速度{self.angularSpeed_F:.3f}')
                    else:
                        print(
                            f'到达目标角度,本次偏差角度{planTheta:.3f},上次偏差角度{self.planTheta_last:.3f}m')
                    self.linearSpeed_T = 0
                    self.angularSpeed_T = 0
                    self.flag_controlSpeed = 1
                    self.sig_updateMotionDone.emit()
                    self.mode_motion = 'stop'
                    self.flag_slowDown = False
                # 接近目标角
                elif planTheta < self.decTheta and self.flag_slowDown == False:
                    print(f'开始减速{planTheta:.3f}')
                    self.linearSpeed_T = 0
                    if self.angularSpeed_F >= 0:
                        self.angularSpeed_T = self.creepangularSpeed
                    else:
                        self.angularSpeed_T = -self.creepangularSpeed
                    self.flag_controlSpeed = 1
                    self.flag_slowDown = True
                self.planTheta_last = planTheta

    def speedControl(self):  # 速度曲线
        if self.flag_controlSpeed:
            # region # 线速度曲线控制
            self.linearSpeed_T = max(
                min(self.linearSpeed_T, self.agv_max_linearSpeed), -self.agv_max_linearSpeed)
            if self.linearSpeed_C == self.linearSpeed_T:
                pass
            elif (self.linearSpeed_C < self.linearSpeed_T) and (self.linearSpeed_C >= 0) and (self.linearSpeed_T >= 0):
                # 当前速度和目标速度同正,正向加速
                self.linearSpeed_C += self.agv_acceleration * self.time_period / 1000
                if self.linearSpeed_C > self.linearSpeed_T:
                    self.linearSpeed_C = self.linearSpeed_T
            elif (self.linearSpeed_C > self.linearSpeed_T) and (self.linearSpeed_C >= 0) and (self.linearSpeed_T >= 0):
                # 当前速度和目标速度同正,正向减速
                self.linearSpeed_C -= self.agv_deceleration * self.time_period / 1000
                if self.linearSpeed_C < self.linearSpeed_T:
                    self.linearSpeed_C = self.linearSpeed_T
            elif (self.linearSpeed_C > self.linearSpeed_T) and (self.linearSpeed_C <= 0) and (self.linearSpeed_T <= 0):
                # 当前速度和目标速度同负,反向加速
                self.linearSpeed_C -= self.agv_acceleration * self.time_period / 1000
                if self.linearSpeed_C < self.linearSpeed_T:
                    self.linearSpeed_C = self.linearSpeed_T
            elif (self.linearSpeed_C < self.linearSpeed_T) and (self.linearSpeed_C <= 0) and (self.linearSpeed_T <= 0):
                # 当前速度和目标速度同负,反向减速
                self.linearSpeed_C += self.agv_deceleration * self.time_period / 1000
                if self.linearSpeed_C > self.linearSpeed_T:
                    self.linearSpeed_C = self.linearSpeed_T
            elif (self.linearSpeed_C < 0) and (self.linearSpeed_T > 0):
                # 当前速度负，目标速度正,先反向减速
                self.linearSpeed_C += self.agv_deceleration * self.time_period / 1000
            elif (self.linearSpeed_C > 0) and (self.linearSpeed_T < 0):
                # 当前速度正，目标速度负,先正向减速
                self.linearSpeed_C -= self.agv_deceleration * self.time_period / 1000
            else:
                print("线速度数据错误;")
            # endregion
            # region # 角速度曲线控制
            self.angularSpeed_T = max(
                min(self.angularSpeed_T, self.agv_max_angularSpeed), -self.agv_max_angularSpeed)
            if self.angularSpeed_C == self.angularSpeed_T:
                pass
            elif (self.angularSpeed_C < self.angularSpeed_T) and (self.angularSpeed_C >= 0) and (self.angularSpeed_T >= 0):
                # 当前速度和目标速度同正,正向加速
                self.angularSpeed_C += self.agv_RateAcceleration * self.time_period / 1000
                if self.angularSpeed_C > self.angularSpeed_T:
                    self.angularSpeed_C = self.angularSpeed_T
            elif (self.angularSpeed_C > self.angularSpeed_T) and (self.angularSpeed_C >= 0) and (self.angularSpeed_T >= 0):
                # 当前速度和目标速度同正,正向减速
                self.angularSpeed_C -= self.agv_RateDeceleration * self.time_period / 1000
                if self.angularSpeed_C < self.angularSpeed_T:
                    self.angularSpeed_C = self.angularSpeed_T
            elif (self.angularSpeed_C > self.angularSpeed_T) and (self.angularSpeed_C <= 0) and (self.angularSpeed_T <= 0):
                # 当前速度和目标速度同负,反向加速
                self.angularSpeed_C -= self.agv_RateAcceleration * self.time_period / 1000
                if self.angularSpeed_C < self.angularSpeed_T:
                    self.angularSpeed_C = self.angularSpeed_T
            elif (self.angularSpeed_C < self.angularSpeed_T) and (self.angularSpeed_C <= 0) and (self.angularSpeed_T <= 0):
                # 当前速度和目标速度同负,反向减速
                self.angularSpeed_C += self.agv_RateDeceleration * self.time_period / 1000
                if self.angularSpeed_C > self.angularSpeed_T:
                    self.angularSpeed_C = self.angularSpeed_T
            elif (self.angularSpeed_C < 0) and (self.angularSpeed_T > 0):
                # 当前速度负，目标速度正,先反向减速
                self.angularSpeed_C += self.agv_RateDeceleration * self.time_period / 1000
            elif (self.angularSpeed_C > 0) and (self.angularSpeed_T < 0):
                # 当前速度正，目标速度负,先正向减速
                self.angularSpeed_C -= self.agv_RateDeceleration * self.time_period / 1000
            else:
                print("角速度数据错误;")
            # endregion
            if self.linearSpeed_C == self.linearSpeed_T and self.angularSpeed_C == self.angularSpeed_T:
                self.flag_controlSpeed = 0
            # region # 速度下发到电机
            if self.MotorDriveType == "CAN":  # 一直发
                self.sig_updateAgvSpeed.emit(
                    self.linearSpeed_C, self.angularSpeed_C)
            elif self.MotorDriveType == "UsbToCAN":  # 变化时发
                if self.linearSpeed_T != self.linearSpeed_T_last or self.angularSpeed_T != self.angularSpeed_T_last:
                    self.sig_updateAgvSpeed.emit(
                        self.linearSpeed_T, self.angularSpeed_T)
                    self.linearSpeed_T_last = self.linearSpeed_T
                    self.angularSpeed_T_last = self.angularSpeed_T
            if self.flag_vel_feedback == 0:
                self.linearSpeed_F = self.linearSpeed_C
                self.angularSpeed_F = self.angularSpeed_C
            # endregion

    def projection_distance(self, A, B, C):  # 目标A，当前B，起点C；计算AB在AC的投影长度
        # 提取点的坐标
        xA, yA = A
        xB, yB = B
        xC, yC = C

        # 计算向量AB和AC
        AB = (xB - xA, yB - yA)
        AC = (xC - xA, yC - yA)

        # 计算点积
        dot_product = AB[0] * AC[0] + AB[1] * AC[1]

        # 计算向量AC的模
        magnitude_AC = math.sqrt(AC[0]**2 + AC[1]**2)

        # 计算叉积
        cross_product = AB[0] * AC[1] - AB[1] * AC[0]

        # 计算向量AC的模
        magnitude_AC = math.sqrt(AC[0]**2 + AC[1]**2)

        # 计算投影距离
        projection_dist = dot_product / magnitude_AC

        # 计算点B到向量AC的距离
        cross_distance = cross_product / magnitude_AC

        return projection_dist, cross_distance

    def calculate_distance(self, x1, y1, x2, y2):
        distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
        return distance

    def calculate_theta(self, x1, y1, x2, y2):
        theta = math.atan2((y2 - y1), (x2 - x1))
        return theta

    def calculate_theta_deviation(self, theta_C, theta_T):
        theta = theta_T - theta_C
        theta = (theta + math.pi) % (2 * math.pi) - math.pi
        return theta

    def updateOdom_PGV(self, x, y, angle, tagnum):
        deviation_x = self.agv_posX - x
        deviation_y = self.agv_posY - y
        # 沿Y轴运动修正角度
        if (self.path_direction > 80 and self.path_direction < 100):
            if abs(deviation_x) > 0.01:
                self.sig_updateQrcode.emit(tagnum, deviation_x)
            if abs(deviation_y) > 0.05:
                print(
                    f'dedeviation_x{round(deviation_x,3)},deviation_y{round(deviation_y,3)},tmp_tagNum{tagnum}')
        elif self.path_direction > -100 and self.path_direction < -80:
            if abs(deviation_x) > 0.01:
                self.sig_updateQrcode.emit(tagnum, -deviation_x)
            if abs(deviation_y) > 0.05:
                print(
                    f'dedeviation_x{round(deviation_x,3)},deviation_y{round(deviation_y,3)},tmp_tagNum{tagnum}')
        self.agv_posX = x
        self.agv_posY = y
        if angle > 180:
            angle -= 360
        elif angle < -180:
            angle += 360
        self.agv_angle = angle
        self.imu_offset = self.agv_angle-self.imu_angle

    def updateIMU(self, angle):
        self.imu_angle = -angle
        self.agv_angle = self.imu_angle + self.imu_offset
        self.agv_theta = self.agv_angle / 180 * math.pi
        self.agv_theta = (
            self.agv_theta + math.pi) % (2 * math.pi) - math.pi
        # print("IMU角度:",self.imu_angle)

    def securityProtect(self, num):
        if self.control_mode != 'mannul':  # 只有在自动模式下才进行安全保护
            print(f"安全保护{num}")
            self.linearSpeed_T = 0
            self.angularSpeed_T = 0
            if num == 2:
                print(f'路径方向{self.path_direction:.3f}°')
                print(f'AGV角度{self.agv_theta:.3f}°')
                print(f'AGV角度{self.agv_angle:.3f}°')
                print(f'航向角{self.heading_angle:.3f}°')
# endregion


class ROSQtNode(QMainWindow, Ui_widget_main.Ui_MainWindow):
    # region
    sig_print = pyqtSignal(str, str, bool)
    # endregion

    def __init__(self):
        super(ROSQtNode, self).__init__()
        self.setupUi(self)
        self.widget_control = widget_control()
        self.gridLayout_control.addWidget(self.widget_control)
        # region # 节点初始化
        rclpy.init(args=None)
        self.ros_node = Node('agv_control_node')
        self.shutdown_event = threading.Event()
        self.thread_ = threading.Thread(target=self._thread_node, daemon=True)
        self.thread_.start()
        self.ros_node.get_logger().info("启动节点...")
        # endregion
        # region # 地图显示
        # script_dir = os.path.dirname(os.path.abspath(__file__))
        # image_path = os.path.join(script_dir, 'maps', 'map06041133.pgm')
        # module_path = inspect.getfile(agv_control.maps)
        # self.print(module_path)
        # image_path = os.path.join(module_path,'map06041133.pgm')
        image_path = f'/home/robot/Desktop/theyn/ros2_foxy_ws/src/agv_control/agv_control/maps/map06041133.pgm'
        # self.image = cv2.imread(image_path)  # 替换为你的图像路径
        self.image = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
        if self.image is None:
            self.get_logger().error("Error: Could not load the background image.")
            return
        # if self.image is not None and not self.image.empty():
        #     self.image_bgr = cv2.cvtColor(self.image, cv2.COLOR_BGR2RGB)
        # else:
        #     self.print("Received empty or invalid image!")
        #     return
        # 转换图像为 RGB 格式
        # self.image_bgr = cv2.cvtColor(self.image, cv2.COLOR_BGR2RGB)
        self.image_bgr = cv2.cvtColor(self.image, cv2.COLOR_GRAY2BGR)
        # 获取图像的尺寸
        height, width, channel = self.image_bgr.shape

        # 将 numpy 数组转换为 QImage
        q_image = QImage(self.image_bgr.data, width, height, width * channel, QImage.Format_RGB888)

        # 将 QImage 转换为 QPixmap
        self.pixmap = QPixmap.fromImage(q_image)

         # 创建一个 QLabel 用于显示图像
        self.image_label = QLabel()
        self.image_label.setAlignment(Qt.AlignCenter)

        # 创建一个 QScrollArea 用于滚动查看图像
        self.scroll_area = QScrollArea()
        self.scroll_area.setWidget(self.image_label)
        self.scroll_area.setWidgetResizable(False)
        self.scroll_area.viewport().installEventFilter(self)
        self.scroll_area.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
        self.scroll_area.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
        self.image_label.setPixmap(self.pixmap)
        self.gridLayout_map.addWidget(self.scroll_area)
        # self.gridLayout_map.addWidget(self.image_label)

        # self.image_label.setScaledContents(True)  # 启用自动缩放
        self.scale_factor = 1.0  # 初始缩放因子
        self.last_mouse_pos = QPoint()  # 用于跟踪鼠标位置
        # endregion
        # region # 初始化参数
        global AgvState
        AgvState = AgvParameters.agvState
        # endregion
        # region # 通讯总线管理
        self.serial_CAN = SerialPort(
            port=DeviceParameters.serial_CAN, baudrate=115200, device_type=0x4341)
        self.serial_CAN.data_received.connect(self.receive_data_handle)
        self.serial_CAN.start()

        self.serial_handle = SerialPort(
            port=DeviceParameters.serial_handle, baudrate=115200, device_type=0x4844)
        self.serial_handle.data_received.connect(self.receive_data_handle)
        self.serial_handle.start()
        # endregion
        # region # 运动模拟
        self.en_simulate = False
        # self.timer_move = QTimer(self)
        # self.timer_move.timeout.connect(self.fuc_motion_simulation)
        # self.timer_move.start(10)
        # threading.Thread(target=self.fuc_motion_simulation).start()
        self.agv_posX = AgvState['agvPosition']['x']
        self.agv_posY = AgvState['agvPosition']['y']
        self.agv_posX_last = AgvState['agvPosition']['x']
        self.agv_posY_last = AgvState['agvPosition']['y']
        self.agv_theta = AgvState['agvPosition']['theta']
        self.agv_angle = self.agv_theta * (180 / math.pi)
        self.widget_control.lineEdit_agv_posX.setText(
            str(round(self.agv_posX, 3)))
        self.widget_control.lineEdit_agv_posY.setText(
            str(round(self.agv_posY, 3)))
        self.widget_control.lineEdit_agv_angle.setText(
            str(round(self.agv_angle, 3)))
        self.linearSpeed_T = 0
        self.angularSpeed_T = 0
        self.movePeriod = 10
        self.multiLinearSpeed = 1  # 线速度倍率(提高模拟效率)
        self.multiangularSpeed = 1  # 角速度倍率(提高模拟效率)
        self.creepSpeed = 0.03  # 爬行速度
        self.decDistance = 0  # 提前减速距离
        self.travelDistance_C = 0  # 当前行驶距离
        self.travelDistance_T = 0  # 目标行驶距离
        self.rotateTheta_C = 0  # 当前旋转角度
        self.rotateTheta_T = 0  # 目标旋转角度
        self.node_num_C = 0  # 当前节点编号
        self.node_num_T = 0  # 所有节点个数
        self.edge_num_C = 0  # 当前节点编号
        self.edge_num_T = 0  # 所有节点个数
        self.done_edge = False  # 是否完成当前线段
        self.mode_motion = 'waiting'  # 等待waiting, 直线运动move, 旋转rotate
        self.allow_deviation = 0.07 * self.multiLinearSpeed  # 距离误差
        self.widget_control.btn_forward.clicked.connect(self.fuc_forward)
        self.widget_control.btn_backward.clicked.connect(self.fuc_backward)
        self.widget_control.btn_turnLeft.clicked.connect(self.fuc_turnLeft)
        self.widget_control.btn_turnRight.clicked.connect(self.fuc_turnRight)
        self.widget_control.btn_stop.clicked.connect(self.fuc_stop)

        # self.btn_clear_Trajectory.clicked.connect(self.clear_Trajectory)
        # self.checkBox_en_simulate.toggled.connect(self.fuc_en_simulate)
        # endregion
        # region # 手柄控制运动
        self.flag_joystickReset = 0  # 遥杆回正时触发标志位
        self.handler_last = {
            'AL1': 0,
            'AL2': 0,
            'AL3': 0,
            'AL4': 0,
            'L1': True,  # 遥杆键
            'LM': True,
            'LU': True,
            'LD': True,
            'LL': True,
            'LR': True,
        }

        self.handler = {
            'AL1': 0,
            'AL2': 0,
            'AL3': 0,
            'AL4': 0,
            'L1': True,  # 遥杆键
            'LM': True,
            'LU': True,
            'LD': True,
            'LL': True,
            'LR': True,
        }
        # endregion
        # region # 里程计&速度曲线控制&纠偏
        self.Odometer = Odometer()
        self.MotorDriveType = "UsbToCAN"
        self.Odometer.MotorDriveType = self.MotorDriveType
        self.Odometer.start()
        self.Odometer.sig_updateAgvPos.connect(self.updateAgvPos)
        self.Odometer.sig_updateAgvSpeed.connect(self.updateAgvSpeed)
        # endregion
        # region # ROS节点管理
        # 发布TF坐标系变换
        self.ros_node.tf_broadcaster = TransformBroadcaster(self.ros_node)
        self.transform_stamped = TransformStamped()
        self.transform_stamped.header.frame_id = "odom"  # 父坐标系
        self.transform_stamped.child_frame_id = "base_footprint"        # 子坐标系

        self.map_to_odom = TransformStamped()
        self.map_to_odom.header.frame_id = "map"
        self.map_to_odom.child_frame_id = "odom"

        # 订阅和发布话题
        self.ros_node.publish_vel_feedback = self.ros_node.create_publisher(Twist, 'vel_feedback', 10)
        self.ros_node.publisher_ = self.ros_node.create_publisher(Int32, 'chatter', 10)
        # self.ros_node.timer = self.ros_node.create_timer(1, self.timer_callback)
        self.call_count = 0  # 初始化调用计数器

        self.ros_node.subscription = self.ros_node.create_subscription(
            Twist,
            '/cmd_vel',  # 假设速度话题是 '/cmd_vel'
            self.echo_speed,
            10
        )
        self.ros_node.sub_imu = self.ros_node.create_subscription(
            Imu,
            '/imu',  # IMU 话题名称
            self.imu_callback,
            10
        )
        self.ros_node.sub_initialpose = self.ros_node.create_subscription(
            PoseWithCovarianceStamped,
            'initialpose',
            self.initialpose_callback,
            10
        )
        self.car_location = Point()
        self.car_location.x = 10.0
        self.car_location.y = 10.0
        self.imu_angle = 0.0
        self.image_total_move = self.image_label.pos()
        self.ros_node.sub_laser = self.ros_node.create_subscription(
            LaserScan,
            '/scan',  # 雷达话题名称，根据实际情况修改
            self.laser_callback,
            10
        )
        # endregion

    # region # 日志管理

    def print(self, text):
        self.ros_node.get_logger().info(text)

    def fuc_log(self, text="", type="", save=False):  # 日志显示区打印
        self.sig_print.emit(text, type, save)

    def sigCall_print(self, text="", classify="", save=False):
        # 定义ANSI转义序列
        red = '\x1b[31m'
        yellow = '\x1b[33m'
        blue = '\x1b[34m'
        reset = '\x1b[0m'  # 重置颜色
        # log = get_time() + "  " + text
        # log=f"{red}{text}{reset}"
        log = f'<span style="color: blue;">{get_time()}</span> {text}'
        # print(text)
        self.ros_node.get_logger().info(text)

        # self.textBrowser.append(log)
        # if save:
        #     self.fuc_savelog(get_time() + '  ' + text, classify)
    # endregion
    # region # 数据打包发送
    def fuc_send_dataRaw(self, Type, data):
        sendData2 = bytes(data)
        if Type == 0x4341:
            self.serial_CAN.write(sendData2)

    def fuc_send_data(self, Type, cmd_id, cmd_data):
        cmd_header = [0x50, 0x43]
        cmd_length = [0x00, 0x00]
        cmd_check = [0x00]
        cmd_end = [0xda]
        cmd_length[1] = len(cmd_data) + 8
        sendData0 = cmd_header + cmd_length + cmd_id + cmd_data
        cmd_check[0] = SumCheck(sendData0, cmd_length[1] - 2)
        sendData1 = sendData0 + cmd_check + cmd_end
        sendData2 = bytes(sendData1)
        if Type == 0x4341 or Type == 0x5043:
            self.serial_CAN.write(sendData2)
    # endregion
    # region # 接收数据处理

    def receive_data_handle(self, identify, device_type, data):  # 接收数据处理
        self.receive_data_restore(identify, device_type, data)
        # print(data)
        # print(identify)
        # print(device_type)

    def receive_data_restore(self, identify, device_type, data):  # 接收数据还原（去转义）
        pattern = rb'(?<!\\);'  # 如果';'前为'\'则不进行分割
        datalist = [x for x in re.split(pattern, data) if x]  # 去掉空''
        for i in datalist:
            pattern = rb'[\\](?=;)'
            i = re.sub(pattern, b"", i)  # 如果'\'后面是';'去掉'\'
            if len(i) > 1:
                # print(len(data))
                self.receive_data_classify(identify, device_type, i)
            else:
                print(data)

    def receive_data_classify(self, identify, device_type, data):  # 接收数据分类
        tmp_buf = [0, 0]
        tmp_buf[0] = device_type >> 8 & 0x00FF
        tmp_buf[1] = device_type & 0x00FF
        # if device_type == 0x4354:
        #     self.deviceStatusChecker.time_lifter = QTime.currentTime()
        # elif device_type == 0x4357:
        #     self.deviceStatusChecker.time_chasis = QTime.currentTime()

        # if ((data[0] == 0x42) & (data[1] == 0x44)):  # BD控制板的数据
        #     self.receive_data_verify(identify, device_type, data)
        # if ((data[0] == 0x43) & (data[1] == 0x41)):  # USB转CAN
        #     self.receive_data_verify(identify, device_type, data)
        # elif ((data[0] == 0x50) & (data[1] == 0x43)):  # PC端数据
        #     self.receive_data_verify(identify, device_type, data)
        # elif ((data[0] == 0x53) & (data[1] == 0x54)):  # ST标准通用
        #     self.receive_data_verify(identify, device_type, data)
        # elif (data[0] == 0x50) or (data[0] == 0x43):  # 接收十六进制数组
        #     a = HexStrAddSpace(data.hex())
        #     self.fuc_log(str(identify) + a, 'data', True)
        if (data[0] == tmp_buf[0] and data[1] == tmp_buf[1]):
            self.receive_data_verify(identify, device_type, data)

        else:  # 接收中文或英文或其它
            try:
                a = data.decode('utf-8')
            except Exception:
                try:
                    a = data.decode('gbk')
                except:
                    a = str(data)
            self.fuc_log(str(identify) + " " + a)
        # except:
        #     self.fuc_log("接收数据分类错误")

    def receive_data_verify(self, identify, device_type, data):  # 接收数据验证
        # print(len(data))
        # if(len(data)>3):
        try:
            # 帧头、帧尾、长度都对
            if ((len(data) > 8) & (len(data) == data[3]) & (data[len(data) - 1] == 0xda)):
                # print("单帧")
                cmdId = data[4] << 8 | data[5]
                self.receive_data_apply(
                    identify, device_type, cmdId, data[6:len(data) - 2])
                # a=HexStrAddSpace(data.hex())#查看原始数据
                # self.fuc_log(str(identify)+a)
            elif ((len(data) < 300) & (len(data) > data[3])):  # 多帧数据拆分开处理
                processData = data[:data[3]]
                # self.fuc_data_verify(processData) #!!!数据合格分开后，重新验证
                # !!!数据合格分开后，重新验证，不能直接调用fuc_data_apply！！！
                self.receive_data_verify(identify, device_type, processData)
                remainData = data[data[3]:]
                # print("多帧")
                # print("总长度"+str(len(data))+'长度'+str(data[3]))
                self.receive_data_classify(identify, device_type, remainData)
                # print("剩余长度"+str(len(remainData)))
            else:
                if (data[len(data) - 1] != 0xda):
                    # self.fuc_log(str(identify)+"帧尾错误")
                    try:
                        print(data)
                        a = data.decode('utf-8')
                        print("english")
                    except Exception:
                        try:
                            # a=data.decode('gbk')
                            a = HexStrAddSpace(data.hex())
                            print("中文")
                        except:
                            a = str(data)
                    # self.fuc_log(str(identify)+HexStrAddSpace(data.hex()))
                    self.fuc_log(str(identify) + a)
                else:
                    print("数据格式错误")
                    # self.fuc_data_apply(data[4:len(data)-2])
        except:
            a = HexStrAddSpace(data.hex())
            self.fuc_log(str(identify) + a)

    def receive_data_apply(self, identify, device_type, cmdId, data):  # 接收数据应用
        if device_type == 0x4341:  # USB转CAN
            if cmdId == 0x4060:  # 速度反馈
                linearSpeed = listToFloat(data[0:4])
                angularSpeed = listToFloat(data[4:8])
                linearSpeed = linearSpeed/DeviceParameters.factor_linearSpeed
                angularSpeed = angularSpeed/DeviceParameters.factor_angularSpeed
                # 发布话题
                twist = Twist()
                twist.linear.x = linearSpeed
                twist.linear.y = angularSpeed
                self.ros_node.publish_vel_feedback.publish(twist)
                # 更新里程计信息
                self.Odometer.linearSpeed_F = linearSpeed
                self.Odometer.angularSpeed_F = angularSpeed
                # print("linearSpeed_F: " + str(linearSpeed) + " angularSpeed_F: " + str(angularSpeed))
                self.widget_control.lineEdit_linearSpeed_C.setText(str(round(linearSpeed, 3)))
                self.widget_control.lineEdit_angularSpeed_C.setText(str(round(angularSpeed, 3)))

        elif device_type == 0x4354:
            self.fuc_log("lifter")
        elif device_type == 0x4357:
            self.fuc_log("chasis")
        elif device_type == 0x4844:  # 手柄
            if cmdId == 0x9001:
                self.handler_apply(data)
                # self.fuc_log("handle")
        elif device_type == 0x5043:  # PC端数据
            if cmdId == 0x4060:  # 速度
                Linear_velocity = listToFloat(data[0:4])
                Angular_velocity = listToFloat(data[4:8])
                self.Odometer.linearSpeed_T = Linear_velocity
                self.Odometer.angularSpeed_T = Angular_velocity
                self.Odometer.flag_controlSpeed = 1
                self.fuc_send_dataRaw(0x4341, data[:8])
        elif device_type == 0x4359:
            pass

    def analysisPGV(self, client_address, device_type, i_PGV_Data):
        # print(i_PGV_Data)
        # 初始化变量
        flag_tagChange = 0
        tmp_tagNum = 0
        tmp_posX = 0
        tmp_posY = 0
        tmp_posYaw = 0
        tmp_agv_imuAngle = 0.0

        # posX
        if i_PGV_Data[2] & 0x04:
            # posX是负数
            tmp_posX = (0xffffff - ((i_PGV_Data[5] & 0x7f) | ((i_PGV_Data[4] & 0x7f) << 7) | ((i_PGV_Data[3] & 0x7f) << 14) |
                                    ((i_PGV_Data[2] & 0x07) << 21))) * -1
        else:
            # posX是正数
            tmp_posX = (i_PGV_Data[5] & 0x7f) | ((i_PGV_Data[4] & 0x7f) << 7) | (
                (i_PGV_Data[3] & 0x7f) << 14) | ((i_PGV_Data[2] & 0x03) << 21)

        # posY
        if i_PGV_Data[6] & 0x40:
            # posY是负数
            tmp_posY = (
                (~((i_PGV_Data[7] & 0x7f) | ((i_PGV_Data[6] & 0x7f) << 7)) + 1) & 0x3fff) * -1
        else:
            # posY是正数
            tmp_posY = (i_PGV_Data[7] & 0x7f) | ((i_PGV_Data[6] & 0x7f) << 7)

        # posYAW
        if i_PGV_Data[10] & 0x40:
            # posYAW是负数
            tmp_posYaw = ((~((i_PGV_Data[11] & 0x7f) | (
                (i_PGV_Data[10] & 0x7f) << 7)) + 1) & 0x3fff) * -1
        else:
            # posYAW是正数
            tmp_posYaw = (i_PGV_Data[11] & 0x7f) | (
                (i_PGV_Data[10] & 0x7f) << 7)

        tmp_posYaw = 3600 - tmp_posYaw
        tmp_tagNum = (i_PGV_Data[17] & 0x7f) | ((i_PGV_Data[16] & 0x7f) << 7) | (
            (i_PGV_Data[15] & 0x7f) << 14) | ((i_PGV_Data[14] & 0x7f) << 21)

        # 打印调试信息
        # print(f"X位置: {tmp_posX}, Y位置: {tmp_posY}, 角度: {tmp_posYaw}, 编码: {tmp_tagNum}")
        self.lineEdit_PGV_Num.setText(str(tmp_tagNum))
        self.lineEdit_PGV_X.setText(str(tmp_posX))
        self.lineEdit_PGV_Y.setText(str(tmp_posY))
        self.lineEdit_PGV_YAW.setText(str(tmp_posYaw))
        if tmp_tagNum:
            matched_data = self.dmCodes.get(tmp_tagNum)  # 匹配tag值
            if matched_data:
                posX = (matched_data['X(mm)'] + tmp_posX / 10) / 1000
                posY = (matched_data['Y(mm)'] + tmp_posY / 10) / 1000
                posYaw = (matched_data['误差角度(°)'] + tmp_posYaw / 10)
                # print(f"X位置: {posX}, Y位置: {posY}, 角度: {tmp_posYaw}, 编码: {tmp_tagNum}")
                self.Odometer.updateOdom_PGV(posX, posY, posYaw, tmp_tagNum)
            else:
                print("匹配失败")

    def analysisIMU(self, identify, device_type, data):
        # print(identify)#/dev/ttyUSB1
        print(data)
        # check_head = data.hex()
        # # 校验帧头
        # if check_head != FRAME_HEAD:
        #     continue

    def initialpose_callback(self, msg):
        position = msg.pose.pose.position
        orientation = msg.pose.pose.orientation
        self.Odometer.agv_posX = position.x
        self.Odometer.agv_posY = position.y
        q = [orientation.w, orientation.x, orientation.y, orientation.z]
        (roll, pitch, yaw) = tf_transformations.euler_from_quaternion(q)
        self.Odometer.offset_initialpose_theta = roll - self.Odometer.imu_theta
        self.print(
            f"初始位置: {position.x}, {position.y}, {roll},IMU角度: {self.Odometer.imu_theta}, 偏移角度: {self.Odometer.offset_initialpose_theta}")
        self.updateAgvPos(position.x, position.y, roll)
        self.map_to_odom.header.stamp = self.transform_stamped.header.stamp
        self.ros_node.tf_broadcaster.sendTransform(self.map_to_odom)
    def imu_callback(self, msg):
        # 从 IMU 数据中提取四元数
        orientation = msg.orientation
        q = [orientation.w, orientation.x, orientation.y, orientation.z]

        # 将四元数转换为欧拉角 (roll, pitch, yaw)
        (roll, pitch, yaw) = tf_transformations.euler_from_quaternion(q)
        # self.print(f"Roll:{roll} ")
        self.Odometer.imu_theta = roll
        tmp_theta = roll + self.Odometer.offset_initialpose_theta
        self.Odometer.agv_theta = (tmp_theta + math.pi) % (2 * math.pi) - math.pi
        # 创建一个新的四元数，仅包含围绕 Z 轴的旋转
        # 注意：这里我们实际上只关心 yaw，但变换消息需要完整的四元数
        # 为了简化，我们假设只围绕 Z 轴旋转，其他轴为 0
        # 这种情况下，四元数可以简化为围绕 Z 轴旋转的四元数
        # 使用 yaw 创建新的四元数
        q_z_axis = tf_transformations.quaternion_from_euler(roll, 0, 0)

        # 更新变换中的旋转
        self.transform_stamped.transform.rotation.x = q_z_axis[1]
        self.transform_stamped.transform.rotation.y = q_z_axis[2]
        self.transform_stamped.transform.rotation.z = q_z_axis[3]
        self.transform_stamped.transform.rotation.w = q_z_axis[0]

        self.transform_stamped.transform.translation.x = float(self.Odometer.agv_posX)
        self.transform_stamped.transform.translation.y = float(self.Odometer.agv_posY)

        # 设置时间戳和坐标系
        self.transform_stamped.header.stamp = self.ros_node.get_clock().now().to_msg()       

        # 发布变换       
        # self.ros_node.tf_broadcaster.sendTransform(self.transform_stamped)
    
    def laser_callback(self, msg):
        # 将雷达数据转换为点云
        angle_min = msg.angle_min
        angle_max = msg.angle_max
        angle_increment = msg.angle_increment
        ranges = msg.ranges

        # 假设图像中心是 (width/2, height/2)
        height, width = self.image_bgr.shape[:2]
        center_x = self.car_location.x*20
        center_y = self.car_location.y*20

        # 清除之前的点云
        self.image_bgr = cv2.cvtColor(self.image, cv2.COLOR_GRAY2BGR)

        # 绘制点云
        for i, range_val in enumerate(ranges):
            if range_val < msg.range_max and range_val > msg.range_min:
                angle = angle_min + i * angle_increment+self.imu_angle
                x = int(range_val*20 * np.cos(angle)+ center_x) 
                y = int(range_val*20 * np.sin(angle)+ center_y) 

                # 确保点在图像范围内
                if 0 <= x < width and 0 <= y < height:
                    cv2.circle(self.image_bgr, (x, y), 2,
                               (0, 0, 255), -1)  # 绘制红色点

        # 显示结果
        
        height, width, channel = self.image_bgr.shape
        q_image = QImage(self.image_bgr.data, width, height, width * channel, QImage.Format_RGB888)
        self.pixmap = QPixmap.fromImage(q_image)
        scaled_pixmap = self.pixmap.scaled(
            int(self.pixmap.width() * self.scale_factor),
            int(self.pixmap.height() * self.scale_factor),
            Qt.KeepAspectRatio,
            Qt.SmoothTransformation
        )
        self.image_total_move = self.image_label.pos()
        # self.image_label.setPixmap(scaled_pixmap)
        # self.image_label.move(self.image_total_move)
    # endregion

    # region # 界面控制运动

    def fuc_forward(self):
        self.Odometer.linearSpeed_T = abs(
            float(self.widget_control.lineEdit_linearSpeed_T.text()))
        self.Odometer.angularSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1

    def fuc_backward(self):
        self.Odometer.linearSpeed_T = - \
            abs(float(self.widget_control.lineEdit_linearSpeed_T.text()))
        self.Odometer.angularSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1

    def fuc_turnLeft(self):
        self.Odometer.linearSpeed_T = 0
        self.Odometer.angularSpeed_T = abs(
            float(self.widget_control.lineEdit_angularSpeed_T.text()))
        self.Odometer.flag_controlSpeed = 1

    def fuc_turnRight(self):
        self.Odometer.linearSpeed_T = 0
        self.Odometer.angularSpeed_T = - \
            abs(float(self.widget_control.lineEdit_angularSpeed_T.text()))
        self.Odometer.flag_controlSpeed = 1

    def fuc_stop(self):
        self.Odometer.linearSpeed_T = 0
        self.Odometer.angularSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1
    # endregion
    # region # 手柄控制运动

    def handler_apply(self, buf):
        self.handler['AL1'] = buf[0]
        self.handler['AL2'] = buf[1]
        self.handler['AL3'] = buf[2]
        self.handler['AL4'] = buf[3]
        self.handler['LU'] = (buf[4] & 0x80)
        self.handler['LD'] = (buf[4] & 0x40)
        self.handler['LL'] = (buf[4] & 0x20)
        self.handler['LR'] = (buf[4] & 0x10)
        self.handler['LM'] = (buf[4] & 0x08)
        self.handler['L1'] = (buf[4] & 0x04)

        self.handler['RU'] = (buf[5] & 0x80)
        self.handler['RD'] = (buf[5] & 0x40)
        self.handler['RL'] = (buf[5] & 0x20)
        self.handler['RR'] = (buf[5] & 0x10)
        self.handler['RM'] = (buf[5] & 0x08)
        self.handler['R1'] = (buf[5] & 0x04)

        # 遥杆触发与回正
        if (self.handler['AL3'] > 0x7f-0x10) and (self.handler['AL3'] < 0x7f+0x10) and \
                (self.handler['AL2'] > 0x7f-0x10) and (self.handler['AL2'] < 0x7f+0x10):
            if self.flag_joystickReset == 3:  # 遥杆在中间需要恢复0速度情况
                self.flag_joystickReset = 2
                self.linearSpeed_T = 0
                self.angularSpeed_T = 0
        else:
            self.flag_joystickReset = 1
            if self.handler['AL3'] > 0x7f+0x10:
                self.angularSpeed_T = (
                    self.handler['AL3'] - (0x7f + 0x10)) * 0.03
            elif self.handler['AL3'] < 0x7f-0x10:
                self.angularSpeed_T = (
                    self.handler['AL3'] - (0x7f - 0x10)) * 0.03
            if self.handler['AL2'] > 0x7f+0x10:
                self.linearSpeed_T = (self.handler['AL2'] - (0x7f + 0x10)) * 0.03
            elif self.handler['AL2'] < 0x7f-0x10:
                self.linearSpeed_T = (self.handler['AL2'] - (0x7f - 0x10)) * 0.03
        # 下发遥杆速度
        if self.flag_joystickReset == 1 or self.flag_joystickReset == 2:
            if self.flag_joystickReset == 1:
                self.flag_joystickReset = 3
            elif self.flag_joystickReset == 2:
                self.flag_joystickReset = 0
            self.Odometer.linearSpeed_T = self.linearSpeed_T
            self.Odometer.angularSpeed_T = -self.angularSpeed_T
            self.Odometer.flag_controlSpeed = 1

        # 按键按下
        if self.handler_last['L1'] and not self.handler['L1']:
            print("L1")
            self.Odometer.control_mode = 'mannul'
            self.label_control_mode.setText('手动')
            self.fuc_stop()
        elif self.handler_last['LU'] and not self.handler['LU']:
            self.fuc_forward()
        elif self.handler_last['LD'] and not self.handler['LD']:
            self.fuc_backward()
        elif self.handler_last['LL'] and not self.handler['LL']:
            self.fuc_turnLeft()
        elif self.handler_last['LR'] and not self.handler['LR']:
            self.fuc_turnRight()

        # 按键松开
        if (not self.handler_last['LU'] and self.handler['LU']) or \
           (not self.handler_last['LD'] and self.handler['LD']) or \
           (not self.handler_last['LL'] and self.handler['LL']) or \
           (not self.handler_last['LR'] and self.handler['LR']):
            self.fuc_stop()
        self.handler_last = self.handler.copy()

    # endregion
    # region # 话题控制运动
    def echo_speed(self, msg):
        # 从 Twist 消息中提取速度信息
        linear_speed = msg.linear.x
        angular_speed = msg.linear.y
        self.ros_node.get_logger().info(
            f"Updated speed: linear = {linear_speed}, angular = {angular_speed}")
        self.Odometer.linearSpeed_T = msg.linear.x
        self.Odometer.angularSpeed_T = msg.angular.z
        self.Odometer.flag_controlSpeed = 1
    # endregion
    # region # 里程计&速度曲线控制&纠偏

    def updateAgvPos(self, agv_posX, agv_posY, agv_theta):
        # region # 更新AGV位置信息
        # 用于判断距节点距离
        self.agv_posX = agv_posX
        self.agv_posY = agv_posY
        self.agv_angle = agv_theta * (180 / math.pi)
        # 可视化显示
        self.widget_control.lineEdit_agv_posX.setText(str(round(agv_posX, 3)))
        self.widget_control.lineEdit_agv_posY.setText(str(round(agv_posY, 3)))
        degrees = self.agv_angle
        self.widget_control.lineEdit_agv_angle.setText(str(round(degrees, 1)))
        # endregion

    def updateAgvSpeed(self, linearSpeed, angularSpeed):
        if self.MotorDriveType == "CAN":
            self.MotorDrive.control_motor(linearSpeed, angularSpeed)
        elif self.MotorDriveType == "UsbToCAN":
            cmd_id = [0x40, 0x60]
            cmd_data = floatToList(linearSpeed*DeviceParameters.factor_linearSpeed) + \
                floatToList(-angularSpeed*DeviceParameters.factor_angularSpeed)
            if self.en_simulate == False:
                self.fuc_send_data(0x5043, cmd_id, cmd_data)
    # endregion

    def _thread_node(self):
        while not self.shutdown_event.is_set():
            try:
                while rclpy.ok():
                    rclpy.spin(self.ros_node)
            except KeyboardInterrupt:
                self.ros_node.get_logger().info("正在关闭节点...")

    def timer_callback(self):
        self.call_count += 1  # 每次回调时增加计数器
        msg = Int32()
        msg.data = self.call_count  # 使用计数器作为消息数据
        self.ros_node.get_logger().info('Publishing: "hlly%d"' % msg.data)
        self.ros_node.publisher_.publish(msg)

    def eventFilter(self, obj, event):
        # 检查是否是滚轮事件
        if event.type() == QEvent.Wheel:
            return True  # 忽略事件
        return super().eventFilter(obj, event)
    
    def wheelEvent(self, event):
        # 获取鼠标滚动的方向和位置
        delta = event.angleDelta().y()
        mouse_pos = event.pos()

        # 计算缩放因子
        if delta > 0:
            # 放大
            self.scale_factor *= 1.1
        else:
            # 缩小
            self.scale_factor /= 1.1

        # 限制缩放因子范围
        self.scale_factor = max(0.1, min(self.scale_factor, 5.0))

        # 缩放图像
        scaled_pixmap = self.pixmap.scaled(
            int(self.pixmap.width() * self.scale_factor),
            int(self.pixmap.height() * self.scale_factor),
            Qt.KeepAspectRatio,
            Qt.SmoothTransformation
        )

        # 更新 QLabel 的图像
        self.image_label.setPixmap(scaled_pixmap)

        # 确保图像居中
        self.image_label.adjustSize()

    def mousePressEvent(self, event):
        if event.button() == Qt.LeftButton:
            self.last_mouse_pos = event.pos()

    def mouseMoveEvent(self, event):
        if event.buttons() & Qt.LeftButton:
            delta = event.pos() - self.last_mouse_pos
            self.last_mouse_pos = event.pos()

            # 调整 QLabel 的位置以实现拖动效果
            self.image_label.move(self.image_label.pos() + delta)
            # self.image_total_move += delta
    def mouseReleaseEvent(self, event):
        pass  # 可以留空，或者实现其他逻辑


def main(args=None):
    current_exit_code = 1002457844
    app = QApplication(sys.argv)
    window = ROSQtNode()
    window.show()
    try:
        # 运行应用程序的主事件循环
        while rclpy.ok():
            app.processEvents()
    except KeyboardInterrupt:
        # 捕获Ctrl+C异常
        window.ros_node.get_logger().info("Ctrl+C pressed, exiting gracefully...")
    finally:
        # 确保在退出前执行必要的清理
        window.ros_node.get_logger().info("Application is shutting down.")
        window.ros_node.destroy_node()
        rclpy.shutdown()
        sys.exit(0)
    # try:
    #     while True:
    #         # 处理Qt事件
    #         app.processEvents()
    # except KeyboardInterrupt:
    #     window.ros_node.get_logger().info("接收到中断信号，正在关闭节点...")
    #     # window.ros_node.destroy_node()
    #     # rclpy.shutdown()


if __name__ == '__main__':
    main()
